#ifndef CONTROL_H
#define CONTROL_H

#include "communication.h"

struct arm{
    float joint1;
    float joint2;
    float joint3;
    float joint4;
    float joint5;
};

struct variables{
    float gpsX;
    float gpsZ;
    float angle;
};

struct armvar{
    float x;
    float y;
    float z;
    float angle1;
    float angle2;
};

struct wheelspeed{
    float left_front;
    float left_rear;
    float right_front;
    float right_rear;
};

struct controller{
    float angle_P;
    float angle_D;
    float trans_P;
    float trans_D;
    float camera_I;
    float camera_D;
};

struct camera{
    float cameraX;
    float cameraY;
    float cameraZ;
};

struct gripper{
    float gap;
    float force;
};

void recVarFromComm(SGAGV_TC*tc);
void initControl(void);
void initArm(void);
void setArmJoint(float joint1,float joint2,float joint3,float joint4,float joint5);
int angleControl(float tar);
int positionControl(float offset);
void obstacleAvoidance(void);
int arm_ik(float x, float y, float z,float angle1,float angle2);
int receiveTarget(int flag);
void limitWheelSpeed(void);
void resetWheelSpeed(void);
int controlLog(int sockid);
int armWithPosControl(int sockid);
int visualGrab(int sockid);
int grab(float force,int sockid);
int receiveArmTarget(void);
void goFoward(float speed);
void turnLeft(float speed);
void turnRight(float speed);
void tranLeft(float speed);
void tranRight(float speed);
void initArmEnd(void);
#endif // !CONTROL_H